#include "DriveWithJoysticks.h"
#include "WPILib.h"

DriveWithJoysticks::DriveWithJoysticks() {
	// Use requires() here to declare subsystem dependencies
	Requires(drive);
}

// Called just before this Command runs the first time
void DriveWithJoysticks::Initialize() {

}

// Called repeatedly when this Command is scheduled to run
void DriveWithJoysticks::Execute() {
	
	float leftS = oi->getLeftSpeed();
	float rightS = oi->getRightSpeed();
	
	bool pDrive = drive->isPrecissionDriving();
	bool isDrvFwd = direction->isDrivingForward();
	
	SmartDashboard::Log(leftS, "Left Joy");
	SmartDashboard::Log(rightS, "Right Joy");
	SmartDashboard::Log(drive->getDrivingMode(), "Drive Train direction value");
	SmartDashboard::Log(isDrvFwd, "Driving indicator is showing forward");
	SmartDashboard::Log(pDrive, "Precission driving");
	
	oi->setPrecissionDrivingStatus(pDrive);
	
	if(isDrvFwd)
		drive->setNormalMode();
	else
		drive->setReversedMode();
	
	drive->tankDrive(leftS, rightS);
}

// Make this return true when this Command no longer needs to run execute()
bool DriveWithJoysticks::IsFinished() {
	return false;
}

// Called once after isFinished returns true
void DriveWithJoysticks::End() {
	
}

// Called when another command which requires one or more of the same
// subsystems is scheduled to run
void DriveWithJoysticks::Interrupted() {
}
